#include "pmw3901.h"
#include "spi.h"
#include <math.h>
#include <stdlib.h>

#include "debug.h"
#define CSN_PORT (PortB)
#define CSN_PIN (Pin15)
#define CSN_H PORT_SetBits(CSN_PORT, CSN_PIN);
#define CSN_L PORT_ResetBits(CSN_PORT, CSN_PIN);

#if defined(__CC_ARM)
#pragma anon_unions /*用于支持结构体联合体*/
#endif

typedef __packed struct motionBurst_s
{
    __packed union
    {
        uint8_t motion;
        __packed struct
        {
            uint8_t frameFrom0 : 1;
            uint8_t runMode : 2;
            uint8_t reserved1 : 1;
            uint8_t rawFrom0 : 1;
            uint8_t reserved2 : 2;
            uint8_t motionOccured : 1;
        };
    };

    uint8_t observation;
    int16_t deltaX;
    int16_t deltaY;

    uint8_t squal;

    uint8_t rawDataSum;
    uint8_t maxRawData;
    uint8_t minRawData;

    uint16_t shutter;
} motionBurst_t;

// 判断芯片是否存在
static uint8_t existenceFlag = 0;
// 判断芯片是否正常
static uint8_t isOkFlag = 0;

static uint8_t registerRead(unsigned char address)
{
    uint8_t data = 0;
    CSN_L;
    Ddl_Delay1us(20);
    spiRW(address & 0x7f);
    Ddl_Delay1us(20);
    data = spiRW(0x00);
    Ddl_Delay1us(20);
    CSN_H;
    return data;
}

static void registerWrite(unsigned char address, unsigned value)
{
    CSN_L;
    Ddl_Delay1us(20);
    spiRW(address | 0x80);
    Ddl_Delay1us(20);
    spiRW(value);
    Ddl_Delay1us(20);
    CSN_H;
}		
		
void getPmw3901Data(float *pixSum)		
{										
    if (existenceFlag == 0) 			
    { 	
        return; 
    } 	
			
    uint8_t address = 0x16;				
    uint8_t data[30];					
    uint8_t len = sizeof(motionBurst_t);

    CSN_L;

    spiRW(address & 0x7f);
    for (uint8_t i = 0; i < len; i++)
    {
        data[i] = spiRW(0x00);
    }
    CSN_H;

    motionBurst_t motion; 
    memcpy((uint8_t *)&motion, data, len);

    uint16_t realShutter = (motion.shutter >> 8) & 0x0FF;
    realShutter |= (motion.shutter & 0x0ff) << 8;
    motion.shutter = realShutter;

    static uint8_t count;
    if (motion.minRawData == 0 && motion.maxRawData == 0)
    {
        if (count++ > 100 && isOkFlag)
        {
            count = 0;
            isOkFlag = 0;
        }
    }		
    else
    {		
        count = 0;
        isOkFlag = 1;
    }		
			
    int16_t pixelDx = motion.deltaX;
    int16_t pixelDy = motion.deltaY;
 
    if (motion.squal < 25 && motion.shutter > 0x1f00)
    {
        pixelDx = pixelDy = 0;
    }
    // 这里的XY对应值根据实际安装来更改
    if (abs(pixelDx) < 100 && abs(pixelDy) < 100)
    {
		pixSum[1] += pixelDx; 
		pixSum[0] += pixelDy; 
    }
}

static void initRegisters(void)
{
    unsigned char v;
    unsigned char c1, c2;

    registerWrite(0x7F, 0x00);
    registerWrite(0x55, 0x01);
    registerWrite(0x50, 0x07);
    registerWrite(0x7f, 0x0e);
    registerWrite(0x43, 0x10);

    v = registerRead(0x67);
    if ((v & (1 << 7)) == (1 << 7))
    {
        registerWrite(0x48, 0x04);
    }
    else
    {
        registerWrite(0x48, 0x02);
    }
    registerWrite(0x7F, 0x00);
    registerWrite(0x51, 0x7b);
    registerWrite(0x50, 0x00);
    registerWrite(0x55, 0x00);

    registerWrite(0x7F, 0x0e);
    v = registerRead(0x73);
    if (v == 0)
    {
        c1 = registerRead(0x70);
        if (c1 <= 28)
            c1 = c1 + 14;
        else
            c1 = c1 + 11;

        if (c1 > 0x3F)
            c1 = 0x3F;

        c2 = registerRead(0x71);
        c2 = ((unsigned short)c2 * 45) / 100;

        registerWrite(0x7f, 0x00);
        registerWrite(0x61, 0xAD);
        registerWrite(0x51, 0x70);
        registerWrite(0x7f, 0x0e);
        registerWrite(0x70, c1);
        registerWrite(0x71, c2);
    }

    registerWrite(0x7F, 0x00);
    registerWrite(0x61, 0xAD);
    registerWrite(0x7F, 0x03);
    registerWrite(0x40, 0x00);
    registerWrite(0x7F, 0x05);
    registerWrite(0x41, 0xB3);
    registerWrite(0x43, 0xF1);
    registerWrite(0x45, 0x14);
    registerWrite(0x5B, 0x32);
    registerWrite(0x5F, 0x34);
    registerWrite(0x7B, 0x08);
    registerWrite(0x7F, 0x06);
    registerWrite(0x44, 0x1B);
    registerWrite(0x40, 0xBF);
    registerWrite(0x4E, 0x3F);
    registerWrite(0x7F, 0x08);
    registerWrite(0x65, 0x20);
    registerWrite(0x6A, 0x18);
    registerWrite(0x7F, 0x09);
    registerWrite(0x4F, 0xAF);
    registerWrite(0x5F, 0x40);
    registerWrite(0x48, 0x80);
    registerWrite(0x49, 0x80);
    registerWrite(0x57, 0x77);
    registerWrite(0x60, 0x78);
    registerWrite(0x61, 0x78);
    registerWrite(0x62, 0x08);
    registerWrite(0x63, 0x50);
    registerWrite(0x7F, 0x0A);
    registerWrite(0x45, 0x60);
    registerWrite(0x7F, 0x00);
    registerWrite(0x4D, 0x11);
    registerWrite(0x55, 0x80);
    registerWrite(0x74, 0x21);
    registerWrite(0x75, 0x1F);
    registerWrite(0x4A, 0x78);
    registerWrite(0x4B, 0x78);
    registerWrite(0x44, 0x08);
    registerWrite(0x45, 0x50);
    registerWrite(0x64, 0xFF);
    registerWrite(0x65, 0x1F);
    registerWrite(0x7F, 0x14);
    registerWrite(0x65, 0x67);
    registerWrite(0x66, 0x08);
    registerWrite(0x63, 0x70);
    registerWrite(0x7F, 0x15);
    registerWrite(0x48, 0x48);
    registerWrite(0x7F, 0x07);
    registerWrite(0x41, 0x0D);
    registerWrite(0x43, 0x14);
    registerWrite(0x4B, 0x0E);
    registerWrite(0x45, 0x0F);
    registerWrite(0x44, 0x42);
    registerWrite(0x4C, 0x80);
    registerWrite(0x7F, 0x10);
    registerWrite(0x5B, 0x02);
    registerWrite(0x7F, 0x07);
    registerWrite(0x40, 0x41);
    registerWrite(0x70, 0x00);

    Ddl_Delay1ms(10); // delay 10ms

    registerWrite(0x32, 0x44);
    registerWrite(0x7F, 0x07);
    registerWrite(0x40, 0x40);
    registerWrite(0x7F, 0x06);
    registerWrite(0x62, 0xF0);
    registerWrite(0x63, 0x00);
    registerWrite(0x7F, 0x0D);
    registerWrite(0x48, 0xC0);
    registerWrite(0x6F, 0xD5);
    registerWrite(0x7F, 0x00);
    registerWrite(0x5B, 0xA0);
    registerWrite(0x4E, 0xA8);
    registerWrite(0x5A, 0x50);
    registerWrite(0x40, 0x80);
}

/* 初始化光流模块 */
uint8_t pmw3901Init(void)
{
    stc_port_init_t stcPortInit;
    MEM_ZERO_STRUCT(stcPortInit);
    stcPortInit.enPinMode = Pin_Mode_Out;
    PORT_Init(CSN_PORT, CSN_PIN, &stcPortInit);
    CSN_H;

    // 上电复位
    registerWrite(0x3a, 0x5a);
    Ddl_Delay1ms(5);

    uint8_t chipId = registerRead(0);
    uint8_t invChipId = registerRead(0x5f);
    // invChipId = 0xB6
    if (chipId == 0x49)
    {
        registerRead(0x02);
        registerRead(0x03);
        registerRead(0x04);
        registerRead(0x05);
        registerRead(0x06);
        Ddl_Delay1ms(1);

        initRegisters();
        Ddl_Delay1ms(5);
        existenceFlag = 1;
    }
	
	
	/*******修复旋转问题*******/
	
//	uint8_t chip1,chip2;
//	
//    registerWrite(0x7f, 0x05);
//	
//	registerWrite(0x5b, 0x11);
////	chip1= registerRead(0x5b);
//	
//	registerWrite(0x6d, 0x11);
////	chip2= registerRead(0x6d);
//	
//	registerWrite(0x7f,0x00); 
	/*********end********/
	
	
    return existenceFlag;
}

uint8_t pmw3901IsOkFlag(void)
{
    return isOkFlag;
}

void fixRotatStart(void)
{
	/*******修复旋转问题*******/
    registerWrite(0x7f, 0x05);
	registerWrite(0x5b, 0x11);
	
	registerWrite(0x6d, 0x11);
	
	registerWrite(0x7f,0x00); 
}

void fixRotatEnd(void)
{ 
    registerWrite(0x7f, 0x05);
	registerWrite(0x5b, 0x32);
	
	registerWrite(0x6d, 0x32);
	
	registerWrite(0x7f,0x00); 
}











